#include "lpf.h"

/**
 * @brief  互补滤波
 * @param  wc : 截止频率
 * @param  dt : 周期
 * @retval void
 */
void lpf_init(lpf_t *hlpf, float wc,float dt)
{
    float ohm = tanf(((wc * dt) / 2.0f) * 3.141592654f);

    hlpf->b0 = 1.f / (1 + (1.0f / ohm));

    hlpf->b1 = 1.f / (1 + (1.0f / ohm));

    hlpf->a0 =  (1 - (1.0f / ohm)) / (1 + (1.0f / ohm));
}

/**
 * @brief  互补滤波
 * @param  in:输入
 * @retval void
 */
float lpf_apply(lpf_t *hlpf, float xn)
{
    // Y(n) = b0*X(n) + b1*X(n-1) + a0*Y(n-1)
    float yn = hlpf->b0 * xn + hlpf->b1 * hlpf->xn_1 - hlpf->a0 * hlpf->yn_1; 

    hlpf->yn_1 = yn;
    hlpf->xn_1 = xn;

    return yn;
}

/******************* (C) COPYRIGHT 2024 FUYEBING*****END OF FILE************/